/*
 * Copyright (c) 2021, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "ti_msp_dl_config.h"
#include "motor.h"
#include "button.h"
#include "task.h"
#include "pid.h"

/////////////////////////////////////////
//UART例程代码
#define delay_ms(X) delay_cycles((CPUCLK_FREQ / 1000) * (X));
volatile unsigned int delay_times = 0;
volatile unsigned char uart_data = 0;
void uart0_send_char(char ch);
void uart0_send_string(char *str);
// 串口发送单个字符
void uart0_send_char(char ch)
{
    // 当串口0忙的时候等待，不忙的时候再发送传进来的字符
    while (DL_UART_isBusy(UART_0_INST) == true)
        ;
    // 发送单个字符
    DL_UART_Main_transmitData(UART_0_INST, ch);
}
// 串口发送字符串
void uart0_send_string(char *str)
{
    // 当前字符串地址不在结尾 并且 字符串首地址不为空
    while (*str != 0 && str != 0)
    {
        // 发送字符串首地址中的字符，并且在发送完成之后首地址自增
        uart0_send_char(*str++);
    }
}
///////////////////////////////



int startflag=0;
volatile uint32_t gpioA;
volatile uint32_t gpioB;
volatile uint32_t gpioKey;
volatile int32_t gEncoderCountA = 0;
volatile int32_t gEncoderCountB = 0;
float speed = 0;
int reference_angle = 72;//舵机正前方

int receive_flag = 0, is_negative=0;
int16_t rx_buf = 0, cx = 120;//偏差
int16_t position_set=120;//期望位置
float kp=0.20,kp_m=10;//比例系数

Motor motor[2];
Servo turnover;
PidTypeDef turn_pid;
//添加几个任务标志位
int daoku_flag = 0;//倒车任务标志
int cefang_flag = 0;//侧方标志位
int Total_Task_flag = 0;//全程任务标志位
int daoku_tim_flag = 0;//定时器标志位
int cefang_tim_flag = 0;
int quancheng_tim_flag=0;
int task1_tim=0;
int task2_tim=0;
int task3_tim=0;
int go_flag = 0; // 向前直行标志位
int daokuover=0;
int cefangover=0;


int main(void)
{
    SYSCFG_DL_init();
    NVIC_EnableIRQ(GPIOA_INT_IRQn);//开启GPIOA端口中断
    NVIC_EnableIRQ(GPIOB_INT_IRQn);//开启GPIOB端口中断

    //清除定时器中断标志
    NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN);
    //使能定时器中断
    NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
    // 清除串口中断标志
    NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
    // 使能串口中断
    NVIC_EnableIRQ(UART_0_INST_INT_IRQN);

    DL_Timer_startCounter(MOTOR_PWM_INST);
    DL_Timer_startCounter(Servo_PWM_INST);
    //初始化电机
    Motor_Init(&motor[0], MOTOR_PWM_INST, DL_TIMER_CC_0_INDEX, MOTOR_AIN1_PORT, MOTOR_AIN1_PIN, MOTOR_AINI2_PORT, MOTOR_AINI2_PIN);
    Motor_Init(&motor[1], MOTOR_PWM_INST, DL_TIMER_CC_1_INDEX, MOTOR_BIN1_PORT, MOTOR_BIN1_PIN, MOTOR_BIN2_PORT, MOTOR_BIN2_PIN);
    //初始化舵机
    Servo_Init(&turnover, Servo_PWM_INST, DL_TIMER_CC_0_INDEX);
    //初始化转向pid
    PID_Init(&turn_pid, PID_POSITION, 50, 25, 0.5, 0.0, 0.0);

    Servo_Driver(&turnover,reference_angle);
    motor[0].speed=0;
    motor[1].speed=0;
    motor[0].speed_set = 800;
    motor[1].speed_set = 800;
    
    

    while (1)
    {
        key_Proc();
        if ((daoku_flag == 1 && daoku_tim_flag == 1) || (daoku_tim_flag == 1&&Total_Task_flag==1))
        {
            DL_GPIO_setPins(LED1_PORT, LED1_PIN_22_PIN);
            daoku();
            DL_GPIO_clearPins(LED1_PORT, LED1_PIN_22_PIN);
            if(Total_Task_flag==1)
            {
                task3_tim = 0;
                daokuover = 1;
            }
            daoku_flag = 0;
            daoku_tim_flag = 0;
            task1_tim = 0;
            go_flag = 0;
        }
        else if((cefang_flag==1&&cefang_tim_flag==1)||(cefang_tim_flag==1&&Total_Task_flag==1))
        {
            DL_GPIO_setPins(LED1_PORT, LED1_PIN_22_PIN);
            cefang();
            DL_GPIO_clearPins(LED1_PORT, LED1_PIN_22_PIN);
            if (Total_Task_flag == 1)
            {
                cefangover = 1;
            }
            cefang_flag = 0;
            cefang_tim_flag = 0;
            task2_tim = 0;
            go_flag = 0;
        }
        else if(Total_Task_flag==1)
        {
            if(cefangover==1){
            Total_Task_flag = 0;
            daokuover = 0;
            quancheng_tim_flag = 0;
            task3_tim = 0;
            go_flag = 0;
            }
        }
        if(go_flag==1)
        {
            motor[0].speed_set=800+( (position_set - cx) * kp_m) ;
            motor[1].speed_set=800-( (position_set - cx) * kp_m) ;
            Servo_Driver(&turnover, turnover.angle_set);
            Motor_Driver(&motor[0], motor[0].speed_set,1);
            Motor_Driver(&motor[1], motor[1].speed_set,1);   
        }
        else 
        {
            Servo_Driver(&turnover,reference_angle);
            stop();
        }
    }
}


//这里编码器读取好像是不行的？
void GROUP1_IRQHandler(void)
{
    //获取中断信号
    gpioA = DL_GPIO_getEnabledInterruptStatus(GPIO_EncoderA_PORT,GPIO_EncoderA_PIN_0_PIN | GPIO_EncoderA_PIN_1_PIN);
    //如果是GPIO_EncoderA_PIN_0_PIN产生的中断
    if((gpioA & GPIO_EncoderA_PIN_0_PIN) == GPIO_EncoderA_PIN_0_PIN)
    {
        //Pin0上升沿，看Pin1的电平，为低电平则判断为反转，高电平判断为正转
        if(!DL_GPIO_readPins(GPIO_EncoderA_PORT,GPIO_EncoderA_PIN_1_PIN))//P1为低电平
        {
            gEncoderCountA--;
        }
        else//P1为高电平
        {
            gEncoderCountA++;
        }
    }
        //类似于Stm32中编码器模式的AB两相都测，可得到2倍的计数
    else if((gpioA & GPIO_EncoderA_PIN_1_PIN) == GPIO_EncoderA_PIN_1_PIN)
    {
        //Pin1上升沿
        if(!DL_GPIO_readPins(GPIO_EncoderA_PORT,GPIO_EncoderA_PIN_0_PIN))//P0为低电平
        {
            gEncoderCountA++;
        }
        else//P1为高电平
        {
            gEncoderCountA--;
        }
    }
    DL_GPIO_clearInterruptStatus(GPIO_EncoderA_PORT,GPIO_EncoderA_PIN_0_PIN | GPIO_EncoderA_PIN_1_PIN );
    gpioB=0;
    gpioB = DL_GPIO_getEnabledInterruptStatus(GPIO_EncoderB_PIN_2_PORT, GPIO_EncoderB_PIN_2_PIN );
    if ((gpioB & GPIO_EncoderB_PIN_2_PIN) == GPIO_EncoderB_PIN_2_PIN)
    {
        // Pin0上升沿，看Pin1的电平，为低电平则判断为反转，高电平判断为正转
        if (!DL_GPIO_readPins(GPIO_EncoderB_PIN_3_PORT, GPIO_EncoderB_PIN_3_PIN)) // P1为低电平
        {
            gEncoderCountB--;
        }
        else // P1为高电平
        {
            gEncoderCountB++;
        }
    }
    DL_GPIO_clearInterruptStatus(GPIO_EncoderB_PIN_2_PORT, GPIO_EncoderB_PIN_2_PIN);
    // 类似于Stm32中编码器模式的AB两相都测，可得到2倍的计数
    gpioB = DL_GPIO_getEnabledInterruptStatus(GPIO_EncoderB_PIN_3_PORT, GPIO_EncoderB_PIN_3_PIN);
    if ((gpioB & GPIO_EncoderB_PIN_3_PIN) == GPIO_EncoderB_PIN_3_PIN)
    {
        // Pin1上升沿
        if (!DL_GPIO_readPins(GPIO_EncoderB_PIN_2_PORT, GPIO_EncoderB_PIN_2_PIN)) // P0为低电平
        {
            gEncoderCountB++;
        }
        else // P1为高电平
        {
            gEncoderCountB--;
        }
    }
    //最后清除中断标志位
    DL_GPIO_clearInterruptStatus(GPIO_EncoderB_PIN_3_PORT, GPIO_EncoderB_PIN_3_PIN);
}



void TIMER_0_INST_IRQHandler(void)
{
    //如果产生了定时器中断
    switch( DL_TimerG_getPendingInterrupt(TIMER_0_INST) )
    {
        case DL_TIMER_IIDX_ZERO://如果是0溢出中断
        //////////
        //此处为按钮的扫描处理
        key.key_status = DL_GPIO_readPins(KEY_PORT, KEY_PIN_21_PIN);
        switch (key.click_status)
        {
        case 0:
            if (key.key_status == 0)
                key.click_status = 1;
            break;
        case 1:
            if (key.key_status == 0)
            {
                key.click_status = 2;
                key.click_time = 0;
            }
            else
                key.click_status = 0;
            break;
        case 2:
            if (key.key_status == 0)
                key.click_time++;

            else if (key.key_status == 1 && key.click_time >= 70)
            {
                key.long_flag = 1;
                key.click_status = 0;
            }

            else if (key.key_status == 1 && key.click_time < 70)
            {
                switch (key.double_status)
                {
                case 0:
                    key.double_status = 1;
                    key.double_time = 0;
                    break;
                case 1:
                    key.double_flag = 1;
                    key.double_status = 0;
                    break;
                }
                key.click_status = 0;
            }
            break;
        }

        if (key.double_status == 1)
        {
            key.double_time++;
            if (key.double_time >= 30)
            {
                key.single_flag = 1;
                key.double_status = 0;
            }
        }
        //到此处为按钮的处理


        ///////////
        //此处为任务定时处理
        if(daoku_flag==1)
        {
            go_flag = 1;
            task1_tim++;
            if(task1_tim==400)
            {
                go_flag = 0;
                daoku_tim_flag = 1;
            }
        }
        else if(cefang_flag==1)
        {
            go_flag = 1;
            task2_tim++;
            if (task2_tim == 500)
            {
                go_flag = 0;
                cefang_tim_flag = 1;
            }
        }
        else if (Total_Task_flag == 1)
        {
            go_flag = 1;
            task3_tim++;
            if(task3_tim==400&&daokuover==0)
            {
                go_flag=0;
                daoku_tim_flag=1;
            }
            if(task3_tim==615&&cefangover==0&&daokuover==1)
            {
                go_flag=0;
                cefang_tim_flag = 1;
            }
        }

        break;
        

        default://其他的定时器中断
            break;
    }
}


// 串口的中断服务函数
void UART_0_INST_IRQHandler(void)
{
        // 如果产生了串口中断
        switch (DL_UART_getPendingInterrupt(UART_0_INST))
        {
        case DL_UART_IIDX_RX: // 如果是接收中断
            // 接发送过来的数据保存在变量中
            uart_data = DL_UART_Main_receiveData(UART_0_INST);
            //S为数据开始位，E为结束判断，发送的格式应为S+应接收数据+E，例如“S123E”，接收的数据即为123；
            if (receive_flag == 1)
            {
                if (uart_data >= '0' && uart_data <= '9') // 如果是数字
                {
                    rx_buf = rx_buf * 10 + (uart_data - '0'); // 累加数字
                }
                else if (uart_data == '+') // 如果是正号
                {
                    is_negative = 0; // 标记为正数
                }
                else if (uart_data == '-') // 如果是负号
                {
                    is_negative = 1; // 标记为负数
                }
                else if (uart_data == 'E') // 如果是结束标志
                {
                    if (is_negative) // 如果是负数
                    {
                        cx = -rx_buf; // 将结果取负
                    }
                    else
                    {
                        cx = rx_buf; // 正数直接赋值
                    }
                    rx_buf = 0;       // 清空缓冲区
                    receive_flag = 0; // 结束接收
                }
            }
            else if (uart_data == 'S') // 如果是开始标志
            {
                receive_flag = 1; // 开始接收
                is_negative = 0;  // 默认为正数
            }

            // 将保存的数据再发送出去
            uart0_send_char(uart_data);
            break;

        default: // 其他的串口中断
            break;
        }
}